#pragma config(Sensor, in1,    lights,         sensorReflection)
#pragma config(Sensor, in2,    rightline,       sensorLineFollower)
#pragma config(Sensor, in3,    midline,        sensorLineFollower)
#pragma config(Sensor, in4,    leftline,      sensorLineFollower)
#pragma config(Sensor, dgtl12, bump,           sensorTouch)
#pragma config(Motor,  port2,           left,          tmotorVex269, openLoop, reversed)
#pragma config(Motor,  port4,           right,         tmotorVex269, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#define stopped 0
#define forward 1
#define turnright 2
#define turnleft 3
#define offline 4

bool checklight()
{
  int lightnum = SensorValue(lights);
  if (SensorValue(lights) <= 150 )
  {
    return true;
  }
  return false;
}

bool checklineleft()
{
  int leftlinenum = SensorValue(leftline);
  if (SensorValue(leftline) < 1700 )
  {
    return true;
  }
  return false;
}

bool checklinemid()
{
  int midlinenum = SensorValue(midline);
  if (SensorValue(midline) < 1700 )
  {
    return true;
  }
  return false;
}

bool checklineright()
{
  int rightlinenum = SensorValue(rightline);
  if (SensorValue(rightline) < 1700 )
  {
    return true;
  }
  return false;
}

bool checkbump()
{
  if (SensorValue[bump] == 1 )
  {
    return true;
  }
  return false;
}

task main ()
{
  int robotstate = stopped;

  while (true)
  {
    bool light = checklight();
    bool lineleft = checklineleft();
    bool linemid = checklinemid();
    bool lineright = checklineright();
    bool bumpfront = checkbump();
    bool turnedright = false;
    bool turnedleft = false;


    switch (robotstate)
    {
    case stopped:
      if (light)
      {
        motor[left] = 65;
        motor[right] = 127;
        robotstate = forward;
      }
      break;

    case forward:
      if (lineleft)
      {
        motor[right] = 40;
        motor[left] = -40;
        robotstate = turnleft;
      }
      if (lineright)
      {
        motor[left] = 40;
        motor[right] = -40;
        robotstate = turnright;
      }
      break;

    case turnright:
      if (linemid)
      {
        motor[left] = 65;
        motor [right] = 127;
        robotstate = forward;
      }
      break;

    case turnleft:
      if (linemid)
      {
        motor[left] = 65;
        motor [right] = 127;
        robotstate = forward;
      }
      break;

    }
  }
}
